#include <cassert>
#include <chrono>
#include <cmath>
#include <fstream>  // 文件流操作
#include <iostream>
#include <thread>
#include <vector>

#include "common/config.h"
#include "common/data.h"
#include "gpu/esdf_map.h"

std::vector<Polygon> mock_obs_polygon() {
    std::vector<Polygon> polygon_vec;

    {
        std::vector<Point2d> point_vec1;
        point_vec1.emplace_back(5, 5);
        point_vec1.emplace_back(10, 10);
        point_vec1.emplace_back(5, 15);
        point_vec1.emplace_back(13, 25);
        point_vec1.emplace_back(16, 12);
        point_vec1.emplace_back(15, 5);
        polygon_vec.emplace_back(point_vec1);
    }

    {
        std::vector<Point2d> point_vec2;
        point_vec2.emplace_back(3, 40);
        point_vec2.emplace_back(16, 35);
        point_vec2.emplace_back(15, 50);
        polygon_vec.emplace_back(point_vec2);
    }

    {
        std::vector<Point2d> point_vec3;
        point_vec3.emplace_back(15, 65);
        point_vec3.emplace_back(10, 75);
        point_vec3.emplace_back(5, 60);
        polygon_vec.emplace_back(point_vec3);
    }

    {
        std::vector<Point2d> point_vec4;
        point_vec4.emplace_back(2, 90);
        point_vec4.emplace_back(5, 90);
        point_vec4.emplace_back(3, 85);
        polygon_vec.emplace_back(point_vec4);
    }

    //////////////
    {
        std::vector<Point2d> point_vec1;
        point_vec1.emplace_back(45, 5);
        point_vec1.emplace_back(50, 10);
        point_vec1.emplace_back(45, 15);
        point_vec1.emplace_back(53, 25);
        point_vec1.emplace_back(56, 12);
        point_vec1.emplace_back(55, 5);
        polygon_vec.emplace_back(point_vec1);
    }

    {
        std::vector<Point2d> point_vec2;
        point_vec2.emplace_back(43, 40);
        point_vec2.emplace_back(56, 35);
        point_vec2.emplace_back(55, 50);
        polygon_vec.emplace_back(point_vec2);
    }

    {
        std::vector<Point2d> point_vec3;
        point_vec3.emplace_back(55, 65);
        point_vec3.emplace_back(50, 75);
        point_vec3.emplace_back(45, 60);
        polygon_vec.emplace_back(point_vec3);
    }

    {
        std::vector<Point2d> point_vec4;
        point_vec4.emplace_back(62, 90);
        point_vec4.emplace_back(65, 90);
        point_vec4.emplace_back(63, 85);
        polygon_vec.emplace_back(point_vec4);
    }

    std::ofstream out_file("polygon_vec.txt");
    assert(out_file.is_open());

    for (const auto& polygon : polygon_vec) {
        const auto& point_vec = polygon.corner_points();
        for (int i = 0; i < point_vec.size(); i++) {
            auto& point = point_vec[i];
            out_file << "(" << point.x << ", " << point.y << "), ";
        }
        out_file << "\n";
    }
    out_file.close();

    return polygon_vec;
}

int main() {
    std::vector<Polygon> polygon_vec = mock_obs_polygon();

    ESDFMap esdf_map(kMapRowSize, kMapColSize, kResolution);
    int N = 10;

    std::cout << "=========CPU 计算============" << std::endl;
    for (int i = 0; i < N; i++) {
        auto start = std::chrono::high_resolution_clock::now();
        esdf_map.reset();
        esdf_map.init_occupancy_boundary(polygon_vec);

        std::chrono::microseconds duration1;
        auto start1 = std::chrono::high_resolution_clock::now();
        esdf_map.init_occupancy_inside(polygon_vec);
        auto end1 = std::chrono::high_resolution_clock::now();
        duration1 = std::chrono::duration_cast<std::chrono::microseconds>(end1 - start1);

        std::chrono::microseconds duration2;
        auto start2 = std::chrono::high_resolution_clock::now();
        esdf_map.calc_esdf_cpu();
        auto end2 = std::chrono::high_resolution_clock::now();
        duration2 = std::chrono::duration_cast<std::chrono::microseconds>(end2 - start2);

        auto end = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        std::cout << "CPU 总耗时: " << duration.count() 
                  << " 微秒, init_occupancy_inside 耗时: " << duration1.count() 
                  << " 微秒, calc_esdf_cpu 耗时: " << duration2.count()
                  << " 微秒" 
                  << std::endl;
    }

    esdf_map.debug_print_occupancy();
    esdf_map.debug_print_esdf_cpu();


    std::cout << "=========GPU 计算============" << std::endl;
    for(int i=0; i<10; i++){
        auto start = std::chrono::high_resolution_clock::now();
        esdf_map.reset();
        esdf_map.init_occupancy_boundary(polygon_vec);

        std::chrono::microseconds duration1;
        auto start1 = std::chrono::high_resolution_clock::now();
        esdf_map.init_occupancy_inside(polygon_vec);
        auto end1 = std::chrono::high_resolution_clock::now();
        duration1 = std::chrono::duration_cast<std::chrono::microseconds>(end1 - start1);

        std::chrono::microseconds duration2;
        auto start2 = std::chrono::high_resolution_clock::now();
        esdf_map.calc_esdf_gpu();
        auto end2 = std::chrono::high_resolution_clock::now();
        duration2 = std::chrono::duration_cast<std::chrono::microseconds>(end2 - start2);

        auto end = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        std::cout << "GPU 总耗时: " << duration.count() 
                  << " 微秒, init_occupancy_inside 耗时: " << duration1.count() 
                  << " 微秒, calc_esdf_gpu 耗时: " << duration2.count()
                  << " 微秒" 
                  << std::endl;
    }

    esdf_map.debug_print_esdf_gpu();
}
